using System;

namespace Blimpbots.Control {
    class VerticalBlimpController {
        private double iTerm;
        private double setPoint;
        private double blimpHeight;
        private String name;

        private void report(String s) {
            Console.WriteLine("ZC_" + name + ": " + s);
            return;
        }

        public VerticalBlimpController(String _name, double _setPoint, double _blimpHeight) {
            name = _name;
            iTerm = 0;
            blimpHeight = _blimpHeight;
            setPoint = _setPoint;
        }

        public double update(double currentZ, double zVelocity) {
            // return motor setting
            double error = setPoint - currentZ;

            report("error: " + error);

            double iFactor = 0;
            double dFactor = 0;

            double errorForMaxCorrection = 3*blimpHeight;
            double deadband = .5*blimpHeight;

            double signal = error + iTerm * iFactor + zVelocity*dFactor;

            iTerm += error;

            double maxMotorSpeed = 7;

            if (signal > 0 && signal > (deadband / 2.0)) {
                signal -= deadband / 2.0;
                signal = signal * (maxMotorSpeed / errorForMaxCorrection);
            }
            else if (signal < 0 && signal < -1 * (deadband / 2.0)) {
                signal += deadband / 2.0;
                signal = signal * (maxMotorSpeed / errorForMaxCorrection);
            }

            if (signal < -1 * maxMotorSpeed) signal = -1 * maxMotorSpeed;
            if (signal > maxMotorSpeed) signal = maxMotorSpeed;
            return -1*signal;
        }
    }
}